#ifndef _GAUSSJ_H_
#define _GAUSSJ_H_

#include "matrix.h"
#include <vector>

template <class T>
inline void SWAP(T &a, T &b) 
{
  T dum = a;
  a = b;
  b = dum;
}

void gaussj(MatDoub &a, MatDoub &b)
{
  int i, icol, irow, j, k, l, ll, m = a.nrows(), n = b.ncols();
  double big, dum, pivinv;
  std::vector<int> indxc(m), indxr(m), ipiv(m);
  for (j = 0; j < m; j++)
    ipiv[j] = 0;
  for (i = 0; i < m; i++) {
    big = 0.0;
    for (j = 0; j < m; j++)
      if (ipiv[j] != 1)
        for (k = 0; k < m; k++) {
          if (ipiv[k] == 0) {
            if (abs(a[j][k]) >= big) {
              big = abs(a[j][k]);
              irow = j;
              icol = k;
            }
          }
        }
    ++(ipiv[icol]);

    if (irow != icol) {
      /* interchange the pivot onto diagonal */
      for (l = 0; l < m; l++) SWAP(a[irow][l], a[icol][l]);
      for (l = 0; l < n; l++) SWAP(b[irow][l], b[icol][l]);
    }

    indxr[i] = irow;            /* original col and row */
    indxc[i] = icol;

    if (a[icol][icol] == 0.0) throw("gaussj: Singular Matrix!");
    pivinv = 1.0/a[icol][icol];
    a[icol][icol] = 1.0;
    for (l = 0; l < m; l++)
      a[icol][l] *= pivinv;
    for (l = 0; l < n; l++)
      b[icol][l] *= pivinv;

    for ( ll = 0; ll < m; ll++)
      if ( ll != icol) {
        dum = a[ll][icol];
        a[ll][icol] = 0.0;
        for (l = 0; l < m; l++)
          a[ll][l] -= a[icol][l] * dum;
        for (l = 0; l < n; l++)
          b[ll][l] -= b[icol][l] * dum;
      }
  }

  /* unscramble the solution in view of the column interchanges. */
  for (l = m - 1; l >= 0; l--)
    if (indxr[l] != indxc[l])
      for (k = 0; k < m; k++)
        SWAP(a[k][indxc[l]], a[k][indxr[l]]);
  
}

/* without right-hand side, so a is replace by its inverse. */
void gaussj(MatDoub &a) 
{
  MatDoub b(a.nrows(), 0);
  gaussj(a, b);
}


#endif /* _GAUSSJ_H_ */
